#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from customized_msgs.msg import FeedBack  # 确保这个自定义消息正确导入

class JoystickReader(Node):
    def __init__(self):
        super().__init__('joystick_motor_py')
        self.get_logger().info("python Joystick reader node initialized")
        # 声明参数
        self.declare_parameters(
            namespace='',
            parameters=[
                ('axis_mapping.left_stick_x', 0),
                ('axis_mapping.left_stick_y', 1),
                ('axis_mapping.LT', 2),
                ('axis_mapping.RT', 5),
                ('axis_mapping.right_stick_x', 3),
                ('axis_mapping.right_stick_y', 4),
                ('axis_mapping.dpad_up', 7),
                ('axis_mapping.dpad_left', 6),

                ('button_mapping.A', 0),
                ('button_mapping.B', 1),
                ('button_mapping.X', 2),
                ('button_mapping.Y', 3),
                ('button_mapping.LB', 4),
                ('button_mapping.RB', 5),
                ('button_mapping.start', 7),
                ('button_mapping.select', 6),
                ('button_mapping.M1', 9),
                ('button_mapping.M2', 10),
                ('button_mapping.home', 8),
            ]
        )

        # 提取参数映射
        self.axis_map_ = {}
        self.button_map_ = {}

        for name in self._parameters:
            if name.startswith('axis_mapping.'):
                key = name.replace('axis_mapping.', '')
                self.axis_map_[key] = self.get_parameter(name).get_parameter_value().integer_value
            elif name.startswith('button_mapping.'):
                key = name.replace('button_mapping.', '')
                self.button_map_[key] = self.get_parameter(name).get_parameter_value().integer_value

        self.print_param()

        self.joy_sub = self.create_subscription(
            Joy, 'joy', self.joy_callback, 10)

        self.motor_fb_2_pub = self.create_publisher(
            FeedBack, '/motor_feedback/motor2_winch', 10)

        self.last_buttons_ = [0] * 11

        self.get_logger().info("Joystick reader node initialized")

    def print_param(self):
        self.get_logger().info("Axis Mapping:")
        for k, v in self.axis_map_.items():
            self.get_logger().info(f"  {k}: {v}")
        self.get_logger().info("Button Mapping:")
        for k, v in self.button_map_.items():
            self.get_logger().info(f"  {k}: {v}")

    def joy_callback(self, joy):
        for name, index in self.axis_map_.items():
            if 0 <= index < len(joy.axes):
                value = joy.axes[index]
                if index in [2, 5]:
                    if abs(value) < 0.95:
                        self.get_logger().info(f"Mapped Axis {name}: {value:.2f}")
                else:
                    if abs(value) > 0.05:
                        self.get_logger().info(f"Mapped Axis {name}: {value:.2f}")

        for name, index in self.button_map_.items():
            if 0 <= index < len(joy.buttons):
                curr = joy.buttons[index]
                last = self.last_buttons_[index] if index < len(self.last_buttons_) else 0
                if curr == 1 and last == 0:
                    self.get_logger().info(f"Mapped Button {name} PRESSED")
                if index >= len(self.last_buttons_):
                    self.last_buttons_.extend([0] * (index - len(self.last_buttons_) + 1))
                self.last_buttons_[index] = curr

def main(args=None):
    rclpy.init(args=args)
    node = JoystickReader()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    
    main()

